package com.robb19y.rgbdscanner.viewer;

import java.net.ConnectException;
import java.util.ArrayList;
import java.util.Hashtable;

import com.robb19y.rgbdscanner.core.RGBDData;

import coppelia.CharWA;
import coppelia.StringW;
import coppelia.remoteApi;

/**
 * JVRep allows to interact with the V-Rep Simulation Environment. 
 * @author robb19y
 *
 */
public class JVRep {
	private static final int PORT = 19997;
	private static final String HOST = "127.0.0.1";
	
	private remoteApi vrep;
	private int clientID;	
	private Hashtable <ArrayList<RGBDData>,String> table = new Hashtable <ArrayList<RGBDData>,String>();

	
	/**
	 * Constructs an instance of JVRep.
	 * Default values for remote connection are used. (Host - 127.0.0.1 , Port - 19997)
	 * @throws Exception in case the remote server can not be reached. 
	 */
	public JVRep() throws Exception{
		super();
		//Initialize V-Rep connection.
		vrep = new remoteApi();
		vrep.simxFinish(-1);
		clientID = vrep.simxStart(HOST,PORT,true,true,5000,5);
		if(clientID == -1){
			throw new ConnectException();
		}		
	}
	
	/**
	 * Constructs an instance of JVRep. Values for remote connection must be specified.
	 * @param host - host of V-Rep service.
	 * @param port - port of V-Rep service.
	 * @throws Exception in case the remote server can not be reached. 
	 */
	public JVRep(String host,int port) throws Exception{
		super();
		//Initialize V-Rep connection.
		vrep = new remoteApi();
		vrep.simxFinish(-1);
		clientID = vrep.simxStart(host,port,true,true,5000,5);
		if(clientID == -1){
			throw new ConnectException();
		}			
	}
	
	/**
	 * Starts the simulator.
	 */
	public void startSimulation(){
		vrep.simxStartSimulation(clientID, remoteApi.simx_opmode_oneshot_wait);
	}
	
	/**
	 * Stop the simulator.
	 */
	public void stopSimulation(){
		vrep.simxStopSimulation(clientID, remoteApi.simx_opmode_oneshot_wait);
	}
	
	public void addPointCloud(ArrayList<RGBDData> data){
		System.out.println("init");
		StringW returnCode = new StringW("");		
		String addPC = "0;"+data.size();
		for(int i = 0; i < data.size(); i++){

			RGBDData rgbd = data.get(i);
			double[] pos = rgbd.getPosition();
			addPC = addPC.concat(";"+pos[2]+";"+pos[0]+";"+(-pos[1])+";"+rgbd.getRed()+";"+rgbd.getGreen()+";"+rgbd.getBlue());
		}
		//addPC = addPC.replaceAll(".", ",");
		addPC = 	addPC.replace(".", ",");
		//System.out.println(addPC);
		vrep.simxSetStringSignal(clientID, "addPC",addPC,  remoteApi.simx_opmode_oneshot_wait);
		boolean ok = true;
		while(ok){			
			vrep.simxGetStringSignal(clientID, "returnCode", returnCode, remoteApi.simx_opmode_oneshot_wait);
			if(!returnCode.getValue().equals("")){
				ok = false;
				table.put(data, returnCode.getValue());
			}
			
		}
	
	}
	
	public void removePointCloud(ArrayList<RGBDData> data){
		vrep.simxSetStringSignal(clientID, "removePC",table.get(data),  remoteApi.simx_opmode_oneshot_wait);
		table.remove(data);
	}
	



}
